#include <util/delay.h>
#include <util/crc16.h>
#include <avr/interrupt.h>

#include "testcfg.h"
#include "bootldr.h"
#include "KS0108.h"
#include "HD44780.h"
#include "mouse.h"
#include "lines.h"
#include "I2C.h"
#include "I2CHW.h"
#include "74HC595.h"
#include "print.h"

#include "AtmelSerialPorts.h"
#include "SmartCard.h"
//#include "i2cEprom.h"

#include "servos.h"
#include "oscillo.h"

#include "rtc.h"
#include "parsing.h"

#include "WorldBitmap.h"
#include "KnightLore.h"
#include "konami.h"
#include "konami_font.h"
#include "cosmic.h"
#include "konami_font.h"
#include "servicegames.h"
#include "cosmic.h"
#include "interruptrequest.h"
#include "donkeykong.h"
#include "abadia.h"
#include "abadia_inicio.h"

#include "I2cEpromHW.h"
#include "accelerometer.h"
#include "timer.h"


void WriteCom( unsigned char dat );

#define PROG_START         0x0000

//-------------------------------------------------------------

unsigned char dat = '>';





//-------------------------------------------------------------
unsigned int ReadTemperature()
{
	unsigned int temp;

	SetEpromAddress8( 128+16, 0 );
	ReadFromEprom( 2, (unsigned char*)&temp);

	temp = (temp<<8) | (temp>>8);

	return temp;
}

void PrintTemperature()
{
	unsigned int temp = ReadTemperature();

	PrintHex8( CharToBCD( temp>>8 ) );
	Device_PrintChar('.');
	
	char val = temp&0x00ff;
	char frac=0;
	if ( val&128 ) frac+= 50;
	if ( val&64  ) frac+= 25;
	if ( val&32  ) frac+= 12;

	PrintHex8( CharToBCD( frac ) );
}




void PrintNunchuck()
{
	// activate
	unsigned char data = 0x00;
	SetEpromAddress8( 0xa4, 0x40 );
	WriteToEprom( 1, &data);

	_delay_us(500);


	unsigned char temp[6];
	SetEpromAddress8( 0xa4, 0x00 );
	ReadFromEprom( 6, (unsigned char*)&temp);
	for (char i=0;i<6;i++)
	{
		PrintHex8( temp[i] );
	}


/*
	char temp[12];
	SetEpromAddress16( 0xa4, 0x0000 );
	ReadFromEprom( 12, (unsigned char*)&temp);
	for (char i=0;i<12;i++)
	{
		PrintHex8( temp[i] );
	}
*/

}

void PrintMotionPlus()
{
	char data[] = { 0x40, 0x00 };
	PrintHex8( WriteLocation( 0xa4, data, 2 ) );

	char data2[] = { 0x00 };
	PrintHex8( WriteLocation( 0xa4, data2, 1 ) );

	unsigned char temp[6];
	PrintHex8( ReadLocation( 0xa4, temp, 6) );

	Device_PrintChar('-');

	for (char i=0;i<6;i++)
	{
		PrintHex8( temp[i] );
	}
}


void GyroOn()
{
	char data[] = { 0xfe, 0x04 };
	PrintHex8( WriteLocation( 0xa6, data, 2 ) );
}

void GyroOff()
{
}


void Ack(char c)
{
	if ( c==0 )
		Device_PrintChar('!');
}

/*
struct Commands
{
	char *command;
	char params;
	void (*func)();
} ;

Commands com[] = 
{
	{"s1", SetServo1}
}

void ParsePointers( char *s )
{
	while(*s!=0)
	{
		for(char i=0;i<1;i++)
		{
			char inc = Parse( com[i].command, s, NULL);
			if (inc>0)
			{
				s+= inc;
				break;
			}
		}
	}
}
*/

void Servo_ParseString( char *s )
{
	while(*s!=0)
	{
		char inc = 1;
		
		if (*s=='S')
		{
			Servo_Init();
		}
		else if ( *s>='a' && *s<='c' )
		{
			unsigned char pos;
			unsigned char index = *s-'a';
			
			s++;
			inc = Parse( "%i", s, &pos);
			Servo_Set1( index, pos );
			//PrintHex8( pos );
		}
		else if ( *s>='d' && *s<='f' )
		{
			unsigned char pos;
			unsigned char index = *s-'d';
			
			s++;
			inc = Parse( "%i", s, &pos);
			PWM_Set( index, pos );
			//PrintHex8( pos );
		}
		
		s += inc;
	}		

	//Servo_Print();
}

// ISwa6wfew04s.
// ISwa4w00w00s.
// ISwa0w00w08s.
// ISwa1RRRRRrs.
//
//  ISw90w00Sw91Rrs.  //read temperature
//  ISw53wfew04. Sw91Rrs.

//  ISwa4w00w08s.     //init
//  ISwa4w00s.
//  ISwa5RRRRRrs. 
//  ISwa4w00w08Swa5RRRRRRr. //read from eprom
//
void I2C_ParseString( char *s )
{
	
	while(*s!=0)
	{
		char inc = 1;
		if (*s=='I')
			I2C_Init();	
		else if (*s=='S')
			I2C_Start();
		else if (*s=='s')
			I2C_Stop();
		else if (*s=='R')
			PrintHex8( I2C_Read(1) );
		else if (*s=='r')
			PrintHex8( I2C_Read(0) );
		else 
		{
			char v;
			inc = Parse( "w%x", s, &v);
			if ( inc == 0)
			{
				return;				
			}
			
			Ack( I2C_Write( v ) );					

		}

		s += inc;
	}		
}



void ParseSmartCard( char *s )
{
	SmartCard_Reset();

	for ( char i=0;i<4;i++)
	{
		PrintHex8( SmartCard_ReadByte() );
	}


	for(;;)
	{
		unsigned char v;

		if (*s=='S')
		{
			SmartCard_Start();
			s++;
		}
		else if (*s=='s')
		{
			SmartCard_Stop();
			s++;
		}
		else if (*s=='R')
		{
			PrintHex8( SmartCard_ReadByte() );
			s++;
		}
		else if ( *s == 0 )
		{
			return;
		}
		else 
		{
			if ( Parse( "W%x", s, &v) == 3 )
			{
				s+=3;
				SmartCard_WriteByte( v );
			}
			else
			{
				break;
			}
		}
	}		

	Device_PrintStr("error '");
	Device_PrintStr( s );
	Device_PrintStr( "'" );

}

//
//  OdDffoD00.
//  OdDffoDff.
//
void ParsePortsString( char *s )
{
	for(;;)
	{
		if ( *s == 'o' || *s == 'd')
		{
			unsigned char op = 0;
			unsigned char port = 0;
			unsigned char val;

			op = *s;
			s++;

			if ( (*s>='A' && *s<='G')  )
			{
				port = *s;
				s++;

				if ( ParseHex8( s, &val) )
				{
					s+=2;
			
					if ( op == 'd' )
					{
						if (port =='A')
							DDRA = val;
						else if (port =='B')
							DDRB = val;
						else if (port =='C')
							DDRC = val;
						else if (port =='D')
							DDRD = val;
						else if (port =='G')
							DDRG = val;
					}
					else if ( op == 'o' )
					{
						if (port =='A')
							PORTA = val;
						else if (port =='B')
							PORTB = val;
						else if (port =='C')
							PORTC = val;
						else if (port =='D')
							PORTD = val;
						else if (port =='G')
							PORTG = val;
					}
					else
					{
				
						break;
					}
				}
				else
				{
					break;
				}

			} 
			else
			{
				break;
			}
		}
		else if ( *s == 'i' )
		{
			s++;

			if ( *s =='A' )
				PrintHex8( PINA );
			else if ( *s =='B' )
				PrintHex8( PINB );
			else if ( *s =='C' )
				PrintHex8( PINC );
			else if ( *s =='D' )
				PrintHex8( PIND );
			else if ( *s =='G' )
				PrintHex8( PING );
			else
			{
				break;
			}
			s++;
		}
		else if ( *s == 0 )
		{
			return;
		}
		else
		{
			break;
		}
	}		

	Device_PrintStr("error '");
	Device_PrintStr( s );
	Device_PrintStr( "'" );
}


void Serial( char *s )
{
	unsigned char val8;
	unsigned int val16;
	unsigned char inc;

	while(*s!=0)
	{
		inc = Parse( "w%x", s, (char *)&val8 ); 
		if ( inc == 0 )
		{
			inc = Parse( "W%X", s, (char *)&val16 ); 
			if ( inc == 0 )
			{
				Device_PrintStr("err");
				return;
			}

			Shift74HC595_Write16( val16 );
		}
		else 
		{
			Shift74HC595_Write8( val8 );
		}

		s += inc;
	}
}


 /*
       _____
  ____|     |____
 |               |
 | 0  2  4  6  + |
 | 1  3  5  7  - | 
 |_______________|

*/


void ReadIR()
{
	unsigned int i,dist;
	int data[15];

	DDRD  &= ~2;
	PORTD &= ~2;

	_delay_us(4);
	
	Device_PrintStr("waiting...");
	while( (PIND & 2) == 0 )
	{
	}

	for (i=0;i<15;)
	{    
		//wait for port0 to go up
		for (dist=0;dist<65535;dist++)
		{
			if ( (PIND & 2) != 0 )
				break;
		}

		data[i++] = dist;
	
		for (dist=0;dist<65535;dist++)
		{
			if ( (PIND & 2) == 0 )
				break;
		}

		data[i++] = dist;
	}


	for (i=0;i<15;i++)
	{    
		PrintHex16( data[i] );
		Device_PrintChar(' ');
	}

}

void SonarMeasure()
{
	unsigned int dist;

	//send sig
	DDRD  |= 2;
	PORTD |= 2;

	_delay_us(4);

	DDRD  &= ~2;
	PORTD &= ~2;

	_delay_us(4);
	    
	//wait for port0 to go up
	for (dist=0;dist<65535;dist++)
	{
		if ( (PIND & 2) != 0 )
			break;
	}
	
	for (dist=0;dist<65535;dist++)
	{
		if ( (PIND & 2) == 0 )
			break;	
	}

	PrintHex16( dist );
}

void DecodeStream()
{
	DDRD &= ~(1<<2);
	PORTD |= (1<<2);

	unsigned int time[10];
	unsigned int count;

	for(;;)
	{

		/*
		//look for a long pulse		
		caca = 0;
		count = 0;
		for (;;)			
		{

			if ( (PIND & 4) == 0 )
				count = 0;
			
			if ( count == 65535 )
			{
				caca++;
				count = 0;

				if (caca>1)
				{
					caca=0;
					break;
				}
			}
		}
		*/


		for ( char index = 0; index< 10;index++ )
		{

			for ( count = 0; count<65535; count++)			
			{
				if ( (PIND & 4) == 0 )
				break;
			}

			for ( count = 0; count<65535; count++)			
			{
				if ( (PIND & 4) != 0)
				break;
			}

			time[index] = count;
		}

		for ( char index = 0; index< 10;index++ )
		{
			PrintHex16( time[index] );
			Device_PrintChar(' ');
		}

		Device_PrintCRLF();	
	}

}

unsigned char EPROM_peek( unsigned int addr )
{
	DDRA = 0;
	PORTA = 0;
	Shift74HC595_Write16(addr);
	return PINA;
}

void DumpHex( unsigned int addr, unsigned int len, unsigned char (*fnPeek)(unsigned int addr) )
{

	while ( len > 0 )
	{
		unsigned int l = len;
		if (l>16) l = 16;

		PrintHex16( addr );
		Device_PrintChar(' ');

		for( unsigned int j=0;j<l;j++ )
		{
			PrintHex8( fnPeek(addr+j) );
			Device_PrintChar(' ');
		}

		for( unsigned int j=0;j<l;j++ )
		{
			unsigned char c = fnPeek(addr+j);
			if (c<32 || c>=128) 
				c = '.';
			Device_PrintChar(c);
		}

		Device_PrintCRLF();	

		addr +=l; 
		len -= l;

	}

}


char stepper2[]={ 1,3,2,6,4,12,8,9};
char stepper[]={ 1,2,4,8,1,2,4,8};

void PrintPosition(int x0, int y0)
{
	PrintHex16( x0 );
	Device_PrintChar( ',' );
	//PrintHex16( y0 );
	Device_PrintCRLF("");		
}


void setPosition( int x0, int y0)
{
	wdt_reset();
	//PrintPosition(x0, y0);
	//Shift74HC595_Write8( stepper[ x0 & 7 ] | (stepper[ y0 & 7 ]<<4) );

	//cnc from scanners
	Shift74HC595_Write8( stepper[ x0 & 7 ] );
	DDRA = DDRA | 0x0f;
	PORTA = (PORTA & 0xf0) | stepper[ y0 & 7 ];
}


char GetChar()
{
	while( !SerialCOM1_DR() )
	{
		wdt_reset();
	}

	return SerialCOM1_Read();
}

static void (*jump_to_bootloader)(void) = (void *)0xF000;
char buffer[40];

void FillBuffer()
{
	int i =0;
	while(1)
	{
		char c = GetChar();
		if (c=='.')
			break;
		buffer[i++]=c;
	};

	buffer[i]=0;
}


unsigned char HWI2CEPROM_peek( unsigned int addr )
{
	unsigned char c;

	SetEpromAddress16( 0xa0, addr );
	ReadFromEprom( 1, &c);

	return c;
}


/*
static unsigned char Map[] = 

{
	 "dffffffe  "
	 "g  aa  g  "
	 "g h  h g  "
	 "g h  h g  "
	 "g  aa  g  "
	 "g  hh  g  "
	 "g h  h g  "
	 "g      g  "
	 "bffffffc  "
};
*/

void DisplayBitmapFromEprom( unsigned int addr )
{
	unsigned char data[128];

	for (char i=0;i<8;i++)
	{
		SetEpromAddress16( 0xa0, addr );
		ReadFromEprom( 128, data);
		KS0108_RenderArray( 0, i, 128, data);

		addr += 128;
	}
}

void PrintTime()
{
	PrintHex8( CharToBCD(RTC_time.HOUR) );
	Device_PrintChar(':');
	PrintHex8( CharToBCD(RTC_time.MINUTE) );
	Device_PrintChar(':');
	PrintHex8( CharToBCD(RTC_time.SECOND) );
}

int temperatures[128];
int temp_index=0;

void Scroll_AddValue(int v)
{
	temperatures[temp_index++] = v;
	temp_index &= 127;
}

void Scroll_Draw()
{
	int tt = 0;
	for(unsigned char i =0;i<128;i++)
	{
		int ii = (temp_index + i)&127;
        unsigned char t = 63 - ((temperatures[ ii ] - (18<<8) ) >>6);

		if ( t< 0)
			t = 0;

		if (t>63)
			t = 63;

		if ( i == 0 )
			tt = t;

		if ( t>tt )
		{
			KS0108_DrawVerticalLine( i, tt, t );
		}
		else
		{
			KS0108_DrawVerticalLine( i, t, tt );
		}

		tt = t;
	}
}

unsigned int ee_addr;

void OnMinute()
{
    if ( RTC_time.SECOND & 1)
	{
		sbiBF(PORTC,7);
	}
	else
	{
		cbiBF(PORTC,7);
	}

	static int lapse = 1;
	lapse--;
	if (lapse <= 0)
	{
		lapse = 60*5; // 5 minutes
		
		int temp = ReadTemperature();

		Scroll_AddValue( temp );

		Scroll_Draw();

		unsigned char data[4] = { RTC_time.HOUR, RTC_time.MINUTE, temp>>8, temp & 0xff };
		SetEpromAddress16( 0xa0, ee_addr );
		WriteToEprom( 4, data );
		ee_addr += 4;
	}

	KS0108_SetFont( konami_font );
	Device_PrintChar = KS0108_PrintChar8x8;
	KS0108_SetAddr(0,0);	
	PrintTime();
	Device_PrintStr("   ");
	PrintTemperature();

	Device_PrintChar = SerialCOM1_Write;
}

void EraseEprom( unsigned int pos, unsigned int length )
{
	for ( unsigned int i = pos; i< (pos + length); i++ )
	{
		unsigned char c = 0xff;
		SetEpromAddress16( 0xa0, i );
		WriteToEprom( 1, &c );
	}

	for ( unsigned int i = pos; i< (pos + length); i++)
	{
		unsigned char c = 0x00;
		SetEpromAddress16( 0xa0, i );
		WriteToEprom( 1, &c );
		if ( c != 0xff )
		{
			Device_PrintStr("Err: ");		
			PrintHex16( i );
			Device_PrintCRLF();		
			return;
		}
	}
}

void FindFirstEntryInEprom()
{
	ee_addr = 0;
	for(;;)
	{
		unsigned char data[4];
		SetEpromAddress16( 0xa0, ee_addr );
		ReadFromEprom( 4, data );
		
		if ( ( data[0] == 0xff ) && ( data[1] == 0xff ) && ( data[2] == 0xff ) && ( data[3] == 0xff )  )
		{
			break;
		}
		else
		{
			ee_addr+=4;
		}
	}
}

void FillGraphFromEprom()
{
	for( unsigned int i=(ee_addr>128*4)?(ee_addr-128*4):0; i< ee_addr; i+=4 )
	{
		unsigned char data[4];
		SetEpromAddress16( 0xa0, i );
		ReadFromEprom( 4, data );
		/*		
		PrintHex16( i );
		Device_PrintStr(": ");	
		PrintHex8( data[0] );
		PrintHex8( data[1] );
		PrintHex8( data[2] );
		PrintHex8( data[3] );

		Device_PrintStr(" - ");	
		*/
		int aa = data[2];
		aa = (aa<<8) | data[3];

		/*
		PrintHex16( aa );
		Device_PrintCRLF();	
		*/

		Scroll_AddValue( aa );
		Scroll_Draw();
	}
}

void MeasurePulses_Test()
{
	for(;;)
	{
		PrintHex16( GetPulse4() );
		Device_PrintChar(' ');		
		PrintHex16( GetPulse5() );
		Device_PrintChar(' ');		
		PrintHex16( GetPulse6() );
		Device_PrintChar(' ');		
		PrintHex16( GetPulse7() );

		Device_PrintCRLF();		
	}


}


void Tracker_down();

volatile unsigned int timer_counter = 0; 
void Tracker()
{
	timer_counter++;
	setPosition(timer_counter, timer_counter);

	if ( timer_counter > 1000 )
	{
		Timer_SetCallback( Tracker_down );
	}
}

void Tracker_down()
{
	timer_counter--;
	setPosition(timer_counter, timer_counter);

	if ( timer_counter <= 10 )
	{
		Timer_SetCallback( Tracker );
	}
}

void Nothing() {};

int main()
{
    asm volatile("cli": : );

	DDRD = 0xff;
	DDRA = 0xff;



	//Oscillo();
	unsigned int step=1;
	//enable watchdog
	//wdt_enable(WDTO_1S);

	
/*
	char welcome[]= 
	{
		"+-------------------+"
		"+  XCHIP  Software  +"
		"+                   +"
		"+    "__DATE__"    +"
		"+                   +"
		"+      "__TIME__"     +"
		"+                   +"
		"+-------------------+"
	};
*/


	SerialCOM1_Init();
	//KS0108_Init();

	//Servo_Init();
	//PWM_Init();
	Timer_Init();
	Timer_SetMilliSeconds( 31 ); 
	Timer_SetCallback( Tracker );
	//Timer_SetCallback( Nothing );
	Device_PrintChar = SerialCOM1_Write;

	//MeasurePulses_Init();
	Timer_SetMilliSeconds( 1000 ); 
	//Timer_SetMilliSeconds( 500 ); 
	//Timer_SetMilliSeconds( 250 );
	//Timer_SetMilliSeconds( 5 );  

	sei();

	/*
	Device_PrintChar = KS0108_PrintChar;

	for(char c=0;welcome[c]!=0;c++)
	{
		KS0108_Locate(0,c);	
		Device_PrintStr(welcome[c]);
	}
	*/
/*
	for( int i=0;i<8;i++)
	{
		KS0108_PrintArray( 0, i, konami_font, Map);
		_delay_us(1000);
	}
*/



	Device_PrintStr("xchip software");		
	Device_PrintCRLF();	
	Device_PrintStr(__DATE__);	
	Device_PrintCRLF();	
	Device_PrintStr(__TIME__);
	Device_PrintCRLF();	


	//MeasurePulses_Test();



/*
	TimerInit();
	TIMSK |= (1<<OCIE1A);
*/
/*
    OCR1A  = (unsigned int)(1000 * (F_CPU  / (1000.0f)));       
    TCCR1A = 0;                                                           
    TCCR1B = (1 << WGM12)|(1 << CS10);                        

    OCR3A  = (unsigned int)(1000 * (F_CPU  / (1000.0f)));       
    TCCR3A = 0;                                                           
    TCCR3B = (1 << WGM32)|(1 << CS30);                        
*/

	while(1)
	{
		Device_PrintChar('>');		
		dat = GetChar();


		Device_PrintChar(dat);
		Device_PrintCRLF();		

		if (dat == 'b')
		{
			jump_to_bootloader();
		}
		else if (dat == 'x')
		{
			FillBuffer();
			char *buf = buffer;
			buf += Parse( "%I", buf, (char *)&step );
			Timer_SetMilliSeconds( step ); 
		}
		else if (dat == 'e')
		{
			FillBuffer();

			static unsigned int pos = 0;
			static unsigned int length = 256;
			static unsigned char twiaddr = 0xa0;
		
			char *buf = &buffer;
			buf += Parse( "a%x", buf, (char *)&twiaddr );
			buf += Parse( "%X", buf, (char *)&pos );
			buf += Parse( "l%I", buf, (char *)&length );

			if ( *buf == 'w' )  // write to eprom
			{
				unsigned char data[32];
				unsigned char ll;

				while( length>0 )
				{
					ll = length>32?32:length;
						
					for(char i=0;i<ll;i++)
					{
						data[i] = GetChar();
					}

					char res = 0;
					do 
					{
						if ( SetEpromAddress16( twiaddr, pos ) == 1 )
						{
							res = WriteToEprom( ll, data );
						}
					} while( res != ll );

					//PrintHex16( length );

					length -= ll;
					pos += ll;
				}

			}
			else if ( *buf == 'r' )  // read from eprom
			{
				unsigned char data[32];
				unsigned char ll;

				while( length>0 )
				{
					ll = length>32?32:length;
					
					SetEpromAddress16( twiaddr, pos );
					ReadFromEprom( ll, data);
						
					for(char i=0;i<ll;i++)
					{
						Device_PrintChar( data[i] );
					}

					length -= ll;
					pos += ll;
				}
			}
			else if ( *buf == 'c' )  // compute crc
			{
				unsigned char data[32];
				unsigned char ll;

				uint16_t crc = 0xffff;

				while( length>0 )
				{
					ll = length>32?32:length;
					
					SetEpromAddress16( twiaddr, pos );
					ReadFromEprom( ll, data);
						
					for(char i=0;i<ll;i++)
					{
						crc = _crc16_update( crc, data[i] );
					}

					length -= ll;
					pos += ll;
				}

				PrintHex16( crc );
			}
			else if ( *buf == 'z' )
			{
				EraseEprom( pos, length );
			}
			else
			{
				DumpHex( pos, length, HWI2CEPROM_peek );
			}

			pos += length;
		
		}
		else if (dat == 'g')
		{
			//DecodeStream();
			//FillBuffer();
			//ParseSmartCard( buffer ); 

			MeasurePulses_Test();
		}
		else if (dat == 's')
		{
			FillBuffer();
			Servo_ParseString( buffer );
		}
		else if (dat == 'f')
		{
			FindFirstEntryInEprom();

			PrintHex16( ee_addr );
			Device_PrintCRLF();	

			FillGraphFromEprom();

			RTC_init();
			RTC_SetSecondCallback( OnMinute );
		}
		else if (dat == 'T')
		{
			FillBuffer();
			Parse( "%i:%i:%i", buffer, (char *)&RTC_time );
		}
		else if (dat == 'D')
		{
			FillBuffer();
			Parse( "%i/%i/%I", buffer, ((char *)&RTC_time)+3 );
		}
		else if (dat == 'S')
		{
			FillBuffer();
			Serial( buffer );
		}
		else if (dat == 'I')
		{
			FillBuffer();
			I2C_ParseString( buffer ); 
		}
		else if (dat == 'O')
		{
			FillBuffer();
			ParsePortsString( buffer );
		}
		else if (dat == 'i')
		{
			I2C_ParseString( "Sw12w34w56w78w90w11w22w33w44s" ); 
			//DisplayBitmapFromEprom( 0 );
		}
		else if (dat >='0' && dat<='9' )

		{
			//uint8_t *pics[] = { WorldBitmap, KnightLore, konami, konami_font, servicegames, cosmic,interruptrequest, donkeykong, abadia, abadia_inicio };
			uint8_t *pics[] = { WorldBitmap, konami_font, servicegames, cosmic, interruptrequest, donkeykong, };
			 KS0108_RenderBitmap( pics[ dat - '0' ] );

			unsigned char data[128];

			unsigned int addr = 0;

			for (char i=0;i<8;i++)
			{
				for (char j=0;j<128;j++)
				{
					data[j] = pgm_read_word( &pics[ dat - '0' ][addr+j] );
				}

				char res=0;
				do
				{
					if ( SetEpromAddress16( 0xa0, addr ) )
					{
						res = WriteToEprom( 128, data);
					}
				}while( res != 0x80 );
				
				addr+=128;
			}

		}
		else if (dat == ' ')
		{
			for( char i = 0 ;i<32;i++)
			{
				KS0108_DrawVerticalLine( i, i, 64-i );
				KS0108_DrawVerticalLine( 32+i, 32-i, 32+i );
			}
			 
			for (char i=0;i<18;i++)
			{
				char dir = i;
				MouseSync();

				MouseWriteByte(dir);

				PrintHex8( dir	 );
				Device_PrintChar('-');
				PrintHex8( MouseReadByte() );
				Device_PrintCRLF();
			}

		}
		else if ( dat>='a' && dat<='g')  
		{
			Device_PrintChar( ( ( PINA >> ( dat - 'a' ) ) & 1 ) + '0');
		} 
		else if ( dat>='A' && dat<='G')  
		{
			DDRA = 1 << ( dat - 'A' );
		} 
		else if (dat == '?')
		{
			Device_PrintStr("some stuff");		
		}
 		else if ( dat == 'o' )
		{
			Oscillo();
		}
		else if (dat == 'l')
		{
			lineBresenham(0, 0, 10, 5, PrintPosition);
		}
		else if (dat == 'm')
		{
			lineBresenham(0, 0, 26, 26, setPosition);
		}
		else if (dat == 'M')
		{
			lineBresenham(   00,   00, 1000,  120, setPosition);
			lineBresenham( 1000,  120, 1500,  370, setPosition);
			lineBresenham( 1500,  370, 1750,  870, setPosition);
			lineBresenham( 1750,  870, 1870, 1870, setPosition);
			lineBresenham( 1870, 1870,   00,   00, setPosition);
			Shift74HC595_Write8( 0 );	
		}
		else if (dat == '+')
		{
			step++;

			Timer_SetMilliSeconds( 1000/step ); 

			/*
			unsigned char v;
			v = stepper[ step & 7 ] | (stepper[ step & 7 ]<<4);
			Shift74HC595_Write8( v );	
			*/
			PrintHex8( step );
			PrintHex16( 1024/step );
		}
		else if (dat == '-')
		{
			step--;

			Timer_SetMilliSeconds( 1000/step ); 
			/*
			unsigned char v;
			v = stepper[ step & 7 ] | (stepper[ step & 7 ]<<4);
			Shift74HC595_Write8( v );	
			*/
			PrintHex8( step );
			PrintHex16( 1024/step );
		}
		else if (dat == 't')
		{
			PrintTemperature();
		}
		else if (dat == 'n')
		{
			PrintMotionPlus();
			//PrintNunchuck();
			//PrintGyro();
		}
		else if (dat == 'N')
		{
			GyroOn();
		}
		else if (dat == 'T')
		{
		    
			PrintHex8( TCNT0 );
			Device_PrintCRLF();		

			PrintHex8( OCR0 );
			Device_PrintCRLF();		

			PrintHex16( TCNT1 );
			Device_PrintCRLF();		

			PrintHex16( OCR1A );
			Device_PrintCRLF();		

			PrintHex16( TCNT2 );
			Device_PrintCRLF();		

			PrintHex16( OCR2 );
			Device_PrintCRLF();		

			PrintHex16( TCNT3 );
			Device_PrintCRLF();		

			PrintHex16( OCR3A );
			Device_PrintCRLF();		

			PrintHex16( PINC );
			Device_PrintCRLF();		

			PrintHex16( PIND );
			Device_PrintCRLF();		

		}
		else
		{
		  Device_PrintStr("err");		
		}

		Device_PrintCRLF();		

    }
}

//End of file: test.c


